/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
#ifndef MATCH_STORAGE_H_
#define MATCH_STORAGE_H_

#include <stdio.h>
#include <vector>
#include <stdlib.h>
#include <string.h>
#define SIZE_OF_TPOSEMATCH (4*7)

/**
* FIXME: The sorter sometimes leaves the last item unsorted??
* - Fixed with so called GLUDGE - there might be a smarter solution
*/

/**
* A pose match structure. Contains information about pose, its uncertainity (area used by the b&b) and weight
*/
struct TPoseMatch{
	public:
		TPoseMatch(){
			setPose(0,0,0);setArea(0,0,0);weight = 0;
		}
		TPoseMatch(float p[3],float a[3], float w){
			set(p,a,w);
		}
		TPoseMatch(float _x, float _y, float _a, float _ax, float _ay, float _aa,float _w){
			setPose(_x,_y,_a); setArea(_ax,_ay,_aa); weight=_w;
		}
		
		void set(const float p[3],const float _a[3], float w){
			x = p[0];y=p[1];a=p[2]; ax=_a[0]; ay=_a[1]; aa = _a[2];
			weight = w;
			
			StatMeasurementDistSum=0;
			StatNumOutliers=0;
			StatNumGoodMeas=0;
		}
		void setPose(float x_, float y_, float a_){
			x = x_; y = y_; a = a_;
		}
		void setArea(float ax_, float ay_, float aa_){
			ax = ax_; ay = ay_; aa = aa_;
		}
		///Checks if given point is inside the search area
		bool isInside(float _x, float _y, float _a){
			if( _x > x-ax && _x<x+ax && _y > y-ay && _y<y+ay && _a>a-aa && _a < a+aa) return true; 
			return false;
		}
		bool isInside(TPoseMatch pm){
			return isInside(pm.x,pm.y,pm.a);
		}
		
			
		bool operator<(const TPoseMatch &s) {
			if(weight<s.weight) return true; 
			return false;
		}
		bool operator<=(const TPoseMatch &s) {
			if(weight<=s.weight) return true; 
			return false;
		}
		bool operator>(const TPoseMatch &s) {
			if(weight>s.weight) return true; 
			return false;
		}
		bool operator>=(const TPoseMatch &s) {
			if(weight>=s.weight) return true; 
			return false;
		}
	
		float x,y,a;
		float ax,ay,aa;
		double weight;
		
		/**
		* Some statistics for the measurement
		* @param StatMeasurementDistSum is a sum of distances to nearest hits that were accepted (not over the threshold)
		* @param StatNumOutliers is the number of rejected measurements
		* @param StatNumGoodMeas is the number of measurements that fitted
		*/
		float StatMeasurementDistSum;
		int StatNumOutliers;
		int StatNumGoodMeas;
};

/**
* A storage for fixed number of TPoseMatches
* The new pose matches are updated into storage and it takes care that the 
* poses with _lowest_ weight are maintained within
* 
*/
class CMatchStorage{
	public:
		/**
		* Costructor requires the number of poses maintained in this storage
		* @param N_ number of matches
		*/
		CMatchStorage(int N_);
		
		/**
		* Costructor for Net packet input (see setNetPacket())
		* @param *in the pointer to input memory block
		* @param size size of input memory block
		*/
		CMatchStorage(const char *in,unsigned int size);
		
		/**
		* Update a new pose match result into the storage
		*/
		void update(TPoseMatch &pm);
		
		/**
		* Tests if given weight @w is better than worse
		* @param w the weight to be tested
		* @return true if it was, else false
		*/
		bool isBetter(float w);
		
		/**
		* Sorts the storage (smallest weights first)
		**/
		void sort();
		
		/**
		 * Provides the storage as a (char *) memory block
		 * @param *out a pointer to memory containing storage. if NULL the memory is reserved, else the size must be (N*SIZE_OF_TPOSEMATCH)
		 * @return number of bytes in *out 
		 */
		unsigned int getNetPacket(char **out);
		/**
		 * Sets a char* memory block into a storage format
		 * @param *in the pointer to memory that contains the data
		 * @param size the size of data, must be N*SIZE_OF_TPOSEMATCH
		 * @return true if ok, else false
		 */
		bool setNetPacket(const char *in, unsigned int size);
		
		
		std::vector<TPoseMatch> storage;
		
		
	private:
		unsigned int N; 				///< Maximum number of poses 
		unsigned int worst; 		///< The index of worst weight at the moment
		float worstW; ///< The worst value;
		
		void searchWorstIndex();
};

#endif